/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include "CParticleFilter.h"
#include "CLineMapRawScanMCL.h"

void randomLikelihoodFunction(CParticleFilter &filt){
		for(int i=0;i<filt.size;i++){
				filt.Particles[i].lik = rand()/RAND_MAX;
		}
		
}

void saveVirtualScannerData(float *r,float *r_xy,float *xy,int num_meas, bool ifAppend){
		FILE *f;
		int i;
		
		if(r!=NULL){
			if(ifAppend) f=fopen("r.txt","at");
			else f=fopen("r.txt","wt");
			
			for(i=0;i<num_meas;i++){
				fprintf(f,"%.3f ",r[i]);
			}	
			fprintf(f,"\n"); fclose(f);
		}
		if(r_xy!=NULL){
				if(ifAppend) f=fopen("relative.txt","at");
				else f=fopen("relative.txt","wt");
			
				for(i=0;i<num_meas;i++){
						fprintf(f,"%.3f %.3f ",r_xy[2*i],r_xy[2*i+1]);
				}	
				fprintf(f,"\n"); fclose(f);
		}
		if(xy!=NULL){
				if(ifAppend) f=fopen("absolute.txt","at");
				else f=fopen("absolute.txt","wt");
			
				for(i=0;i<num_meas;i++){
						fprintf(f,"%.3f %.3f ",xy[2*i],xy[2*i+1]);
				}	
				fprintf(f,"\n"); fclose(f);
		}
}

int main(void){
		CParticleFilter filt;
		mcl::pose init;
		mcl::pose variance;
		mcl::pose dP;
		TLineMap map;
		float angle;
		
		float r[180];
		float lines[3*180];
		float scan_xy[2*180];
		float scan_xy_rel[2*180];
		
		//map.readMapFile("autMapHandled.txt",1.0/1000.0);
		//file:///home/jari/softa/svgparser/kartta_loppudemo.txt
		
		map.readMapFile("multi-map.txt",1.0);
		/*
		for(int i=0;i<map.NumOfLines;i++){
				map.Lines[i].x1 -= 100;
				map.Lines[i].x2 -= 100;
				map.Lines[i].y1 -= 42;
				map.Lines[i].y2 -= 42;
		}
		*/
		map.saveXML("multi-map.xml","walls", "wall");
		
		return 0;
		
		for(angle=0;angle<2*M_PI;angle+=0.1){
				map.virtualScanner(r, ///<out: distance "measurements"
																		 scan_xy_rel, ///< out: measurements in relative x,y-frame
																		 scan_xy, ///< out: measurements in absolute x,y-frame
																		 lines,  ///< out: The line parameters of the scan
																		 0,0,angle,    ///< Position where the scan is taken from
																		 180, ///<Number of measurements taken
																		 0);         ///<offset between laser and robot		
		
				saveVirtualScannerData(r,scan_xy_rel,scan_xy,180, true);
		}
		init.set(0,0,M_PI/4);
		variance.set(0.1,0.1,0.0);
		dP.set(0.1,0,0);
		
		//filt.initializeNormalRandom(init_pose, variance,size_of_filter);
		filt.initializeNormalRandom(init, variance,500);
		filt.print();
		variance.set(0.1,0.1,0.0);
		for(int i=0;i<10;i++){
				filt.predict(dP,variance);
				filt.print();
		}
		randomLikelihoodFunction(filt);
		filt.normalize();
		filt.SIRUpdate();
		filt.print();
		
		
	return 0;
}
